

#include "Elem_Solid_2D_QUAD4.h"
#include "GeomDataLagrange.h"
#include "SolutionDataSolid.h"
#include "QuadratureUtil.h"
#include "TimeFunction.h"
#include "stressrecovery.h"
#include "MyTime.h"
#include "TimeFunction.h"
#include "utilitiesmaterial.h"


using namespace std;


extern  std::vector<unique_ptr<TimeFunction> > timeFunction;
extern  MyTime                 myTime;
extern  bool  debug;



Elem_Solid_2D_QUAD4::Elem_Solid_2D_QUAD4()
{
  if (debug) cout << " constructor Elem_Solid_2D_QUAD4 \n\n";
  
  ndim = 2;
  ndof = 2;
  npElem = 4;
  nsize  = npElem*ndof;

  ELEM_SHAPE = ELEM_SHAPE_QUAD;
}




Elem_Solid_2D_QUAD4::~Elem_Solid_2D_QUAD4()
{
}



void Elem_Solid_2D_QUAD4::prepareElemData()
{
    ElementBase::prepareElemData();

    if(ELEM_FORMULATION == "Bbar")
    {
      Nc.resize(npElem);
      dNc_dx.resize(npElem);
      dNc_dy.resize(npElem);
    }

    return;
}




double Elem_Solid_2D_QUAD4::computeVolume(bool init)
{
    int  ii, jj, gp;
    double  dvol, Jac, param[2];

    VectorXd  N(npElem), dN_dx(npElem), dN_dy(npElem);

    double xNode[npElem], yNode[npElem], xx, yy;
    for(ii=0;ii<npElem;ii++)
    {
      xNode[ii] = GeomData->NodePosOrigLocal[nodeInLocalElem[ii]][0];
      yNode[ii] = GeomData->NodePosOrigLocal[nodeInLocalElem[ii]][1];
    }

    vector<double>  gausspoints1, gausspoints2, gaussweights;
    getGaussPointsQuad(nGP, gausspoints1, gausspoints2, gaussweights);

    bool   axsy  = (GeomData->getModelBehaviour() == 3);
    double thick = GeomData->getThickness();

    elemVol = 0.0;
    for(gp=0; gp<nGP; gp++)
    {
        param[0] = gausspoints1[gp];
        param[1] = gausspoints2[gp];

        if(init)
          GeomData->computeBasisFunctions2D(CONFIG_ORIGINAL, ELEM_SHAPE_QUAD, param, nodeNums, &N(0), &dN_dx(0), &dN_dy(0), Jac);
        else
          GeomData->computeBasisFunctions2D(CONFIG_DEFORMED, ELEM_SHAPE_QUAD, param, nodeNums, &N(0), &dN_dx(0), &dN_dy(0), Jac);

         xx = yy= 0.0;
         for(ii=0;ii<npElem;ii++)
         {
           xx += N[ii]*xNode[ii];
           yy += N[ii]*yNode[ii];
         }

         dvol = gaussweights[gp]*(Jac*thick);

         if(axsy)
           dvol *= 2.0*PI*xx;

         elemVol += dvol;
    }

    return  elemVol;
}



int Elem_Solid_2D_QUAD4::calcMassMatrix(MatrixXd& Mlocal, bool MassLumping)
{
  // mass lumping - row-wise sum
  if(MassLumping)
  {
      double fact  = MatlData->getDensity()*computeVolume(true)/4.0;

      if(Mlocal.rows() != nsize)
        Mlocal.resize(nsize, nsize);
      Mlocal.setZero();

      for(int ii=0; ii<nsize; ii++)
      {
        Mlocal(ii,ii) = fact;
      }
  }
  else
  {
    int  ii, jj, gp, TI, TIp1, TIp2, TJ, TJp1, TJp2;

    VectorXd  N(npElem), dN_dx(npElem), dN_dy(npElem);

    double  fact, dvol, Jac, bb1, cc1, param[3];
    double rho0  = MatlData->getDensity() ;

    double xNode[4], yNode[4], xx, yy;
    for(ii=0;ii<npElem;ii++)
    {
      xNode[ii] = GeomData->NodePosOrigLocal[nodeInLocalElem[ii]][0];
      yNode[ii] = GeomData->NodePosOrigLocal[nodeInLocalElem[ii]][1];
    }

    if(Mlocal.rows() != nsize)
      Mlocal.resize(nsize, nsize);
    Mlocal.setZero();

    vector<double>  gausspoints1, gausspoints2, gaussweights;
    int nGPt=9;
    getGaussPointsQuad(nGPt, gausspoints1, gausspoints2, gaussweights);

    bool   axsy  = (GeomData->getModelBehaviour() == 3);
    double thick = GeomData->getThickness();

    for(gp=0; gp<nGPt; gp++)
    {
        param[0] = gausspoints1[gp];
        param[1] = gausspoints2[gp];

        GeomData->computeBasisFunctions2D(CONFIG_ORIGINAL, ELEM_SHAPE_QUAD, param, nodeNums, &N(0), &dN_dx(0), &dN_dy(0), Jac);

        xx = yy= 0.0;
        for(ii=0;ii<npElem;ii++)
        {
          xx += N[ii]*xNode[ii];
          yy += N[ii]*yNode[ii];
        }

        dvol = gaussweights[gp]*(Jac*thick);

        if(axsy)
          dvol *= 2.0*PI*xx;

        for(ii=0;ii<npElem;ii++)
        {
          bb1 = (dvol*rho0)*N[ii];

          TI   = 2*ii;
          TIp1 = TI+1;

          for(jj=0; jj<npElem; jj++)
          {
              TJ   = 2*jj;
              TJp1 = TJ+1;

              fact  = bb1*N[jj];

              Mlocal(TI,   TJ)    += fact ;
              Mlocal(TIp1, TJp1)  += fact ;
          }
        }
    } //gp
    //cout << " elemVol = " << elemVol << endl;
    //printMatrix(Klocal);  printf("\n\n\n");
  }

  return 0;
}


double  Elem_Solid_2D_QUAD4::calcCriticalTimeStep(bool flag)
{
  double  dtCric;

  return  dtCric;
}



int Elem_Solid_2D_QUAD4::calcStiffnessAndResidual(MatrixXd& Klocal, VectorXd& Flocal)
{
    if(ELEM_FORMULATION == "Displacement")
      return ElementBase::calcStiffnessAndResidual_Formulation_Displacement_2D(Klocal, Flocal);
    else if(ELEM_FORMULATION == "Bbar")
      return calcStiffnessAndResidual_Formulation_Bbar(Klocal, Flocal);
    else if(ELEM_FORMULATION == "Mixed")
      return calcStiffnessAndResidual_Formulation_Mixed(Klocal, Flocal);

    return 1;
}









int Elem_Solid_2D_QUAD4::calcStiffnessAndResidual_Formulation_Bbar(MatrixXd& Klocal, VectorXd& Flocal)
{
    int   err, index, ii, jj, gp, TI, TIp1, TJ, TJp1;

    VectorXd  N(npElem), dN_dx(npElem), dN_dy(npElem);

    double  detF, detFc, fact, dvol, dvol0, Jac, volstrain, pres;
    double  bb1, bb2, bb3, bb4, bb5, cc1, cc2, cc3, cc4, cc5;
    double  param[2], bforce[2]={0.0,0.0}, force[2];
    double  veloCur[2], acceCur[2], sig[2];


    int    sss   = GeomData->getModelBehaviour();
    double thick = GeomData->getThickness();
    bool   axsy  = (sss == 3);
    bool  finite = MatlData->isFiniteStrain();

    double rho0  = MatlData->getDensity() ;
    double rho   = rho0;

    //bforce[0]   = elmDat[6]*timeFunction[0]->getFactor() ;
    //bforce[1]   = elmDat[7]*timeFunction[0]->getFactor() ;

    double af   = SolnData->td(2);
    double acceFact1 = SolnData->td(5);
    double acceFact2 = acceFact1;
    double FiniteFact = (finite == true) ? 1.0 : 0.0;

    MatrixXd  Cmat(9,9), Bmat(3,9), Gc(3,9), F(3,3), Fc(3,3);
    Bmat.setZero();
    Cmat.setZero();
    Gc.setZero();
    F.setZero();
    Fc.setZero();
    VectorXd  stre(9);
    stre.setZero();



    double xNode[npElem], yNode[npElem], xx, yy;
    for(ii=0;ii<npElem;ii++)
    {
      xNode[ii] = GeomData->NodePosOrigLocal[nodeInLocalElem[ii]][0];
      yNode[ii] = GeomData->NodePosOrigLocal[nodeInLocalElem[ii]][1];
    }


    int nGP=1;
    vector<double>  gausspoints1, gausspoints2, gaussweights;


    // calculate values at the centroid
    if(npElem == 6)
    {
      getGaussPointsTriangle(nGP, gausspoints1, gausspoints2, gaussweights);
    }
    else if( (npElem == 4) || (npElem == 9) )
    {
      getGaussPointsQuad(nGP, gausspoints1, gausspoints2, gaussweights);
    }

    param[0] = gausspoints1[0];
    param[1] = gausspoints2[0];

    GeomData->computeBasisFunctions2D(CONFIG_ORIGINAL, ELEM_SHAPE, param, nodeNums, &Nc(0), &dNc_dx(0), &dNc_dy(0), Jac);

    computeDefGrad2DCur(dNc_dx, dNc_dy, Fc);

    detFc = Fc(0,0)*Fc(1,1) - Fc(0,1)*Fc(1,0);


    if(Klocal.rows() != nsize)
    {
      Klocal.resize(nsize, nsize);
      Flocal.resize(nsize);
    }
    Klocal.setZero();
    Flocal.setZero();


    if(npElem == 3)
    {
      nGP = 1;
      getGaussPointsTriangle(nGP, gausspoints1, gausspoints2, gaussweights);
    }
    else if(npElem == 4)
    {
      nGP = 4;
      getGaussPointsQuad(nGP, gausspoints1, gausspoints2, gaussweights);
    }
    else if(npElem == 6)
    {
      nGP = 3;
      getGaussPointsTriangle(nGP, gausspoints1, gausspoints2, gaussweights);
    }
    else if(npElem == 9)
    {
      nGP = 9;
      getGaussPointsQuad(nGP, gausspoints1, gausspoints2, gaussweights);
    }




    for(gp=0; gp<nGP; gp++)
    {
        param[0] = gausspoints1[gp];
        param[1] = gausspoints2[gp];

        GeomData->computeBasisFunctions2D(CONFIG_ORIGINAL, ELEM_SHAPE, param, nodeNums, &N(0), &dN_dx(0), &dN_dy(0), Jac);

        dvol0 = gaussweights[gp]*(thick*Jac);

        xx = yy = 0.0;
        for(ii=0;ii<npElem;ii++)
        {
          xx += N[ii]*xNode[ii];
          yy += N[ii]*yNode[ii];
        }

        if(axsy)
          dvol0 *= 2.0*PI*xx;

        dvol  = dvol0;

        computeDefGrad2DCur(dN_dx, dN_dy, F);

        detF = F(0,0)*F(1,1) - F(0,1)*F(1,0);

        if(detF < 0.0)
        {
          throw runtime_error("Negative Jacobian in the element");
        }

        volstrain = F(0,0) + F(1,1) - 2.0;

        // ADJUST F33 fOR 2D PROBLEMS BASED ON THE ASSUMPTIONS OF PLANE STRESS/PLANE STRAIN/AXISYMMETRIC
        //adjust_deformationgradient_2D(sss, finite, F);

        fact = (Fc(0,0) + Fc(1,1) - F(0,0) - F(1,1))/3.0;

        F(0,0) += fact ;
        F(1,1) += fact ;
        F(2,2)  = 1.0 + fact ;

        //acceCur[0] = computeValueDotDotCur(0, N);
        //acceCur[1] = computeValueDotDotCur(1, N);

        if(finite)
        {
          GeomData->computeBasisFunctions2D(CONFIG_DEFORMED, ELEM_SHAPE, param, nodeNums, &N(0), &dN_dx(0), &dN_dy(0), Jac);
          dvol = gaussweights[gp]*(thick*Jac);
        }

        MatlData->computeStressAndTangent(true, sss, F, F, pres, stre, Cmat, ivar, gp, myTime.dt);
        //if(err !=0)    return 1;
        //printMatrix(Cmat);


        // Calculate Stiffness and Residual
        //==============================================

        force[0] = rho0*bforce[0];
        force[1] = rho0*bforce[1];

        force[0] -= rho0*acceCur[0];
        force[1] -= rho0*acceCur[1];

        //   part 1. -- material part (not necessarily symmetric!!)
        for(ii=0;ii<npElem;ii++)
        {
          bb1 = dvol*dN_dx[ii];
          bb2 = dvol*dN_dy[ii];
          bb3 = dvol*N[ii];
          bb5 = dvol0*N[ii];

          TI   = 2*ii;
          TIp1 = TI+1;

          Gc(0,0) = bb1 * Cmat(0,0) + bb2 * Cmat(3,0);
          Gc(0,1) = bb1 * Cmat(0,1) + bb2 * Cmat(3,1);
          Gc(0,3) = bb1 * Cmat(0,3) + bb2 * Cmat(3,3);
          Gc(0,4) = bb1 * Cmat(0,4) + bb2 * Cmat(3,4);

          Gc(1,0) = bb1 * Cmat(1,0) + bb2 * Cmat(4,0);
          Gc(1,1) = bb1 * Cmat(1,1) + bb2 * Cmat(4,1);
          Gc(1,3) = bb1 * Cmat(1,3) + bb2 * Cmat(4,3);
          Gc(1,4) = bb1 * Cmat(1,4) + bb2 * Cmat(4,4);

          sig[0] = bb1*stre[0] + bb2*stre[3];
          sig[1] = bb1*stre[1] + bb2*stre[4];

          Flocal(TI)   += (bb5*force[0] - sig[0]) ;
          Flocal(TIp1) += (bb5*force[1] - sig[1]) ;

          for(jj=0; jj<npElem; jj++)
          {
              cc1 = dN_dx[jj];
              cc2 = dN_dy[jj];
              cc5 = N[jj];

              TJ   = 2*jj;
              TJp1 = TJ+1;

              // acceleration term
              fact  = bb5*acceFact1*cc5*rho0;

              Klocal(TI,   TJ)    += fact ;
              Klocal(TIp1, TJp1)  += fact ;

              Klocal(TI,   TJ)    +=  af*(Gc(0,0) * cc1 + Gc(0,3) * cc2) ;
              Klocal(TI,   TJp1)  +=  af*(Gc(0,1) * cc1 + Gc(0,4) * cc2) ;
              Klocal(TIp1, TJ)    +=  af*(Gc(1,0) * cc1 + Gc(1,3) * cc2) ;
              Klocal(TIp1, TJp1)  +=  af*(Gc(1,1) * cc1 + Gc(1,4) * cc2) ;
          }
        }
    } //gp

    //printMatrix(Klocal);  printf("\n\n\n");  printVector(Flocal);

    return 0;
}





int Elem_Solid_2D_QUAD4::calcStiffnessAndResidual_Formulation_Mixed(MatrixXd& Klocal, VectorXd& Flocal)
{
    return 0;
}





int Elem_Solid_2D_QUAD4::calcResidual(VectorXd& Flocal)
{
    return 0;
}




void Elem_Solid_2D_QUAD4::elementContourplot(int vartype, int varindex, int index)
{
   double outval[50];

   switch(vartype)
   {
       case 1:  // plot total strain
       case 2:  // plot elastic strain
       case 3:  // plot plastic strain

                projectStrain(true, vartype, varindex, index, outval);

              break;

       case 4:  // plot stress

                projectStress(true, vartype, varindex, index, outval);

              break;

       case 5:  // plot element internal variables

                projectInternalVariable(true, vartype, varindex, index, outval);

              break;

       default:

              cout  << " Invalid Variable Type to project in 'BernsteinElem2DSolidTria6Node::projectToNodes'" << endl;
              break;
    }

    assert(vals2project.size() >= 1);

    vals2project[0] = 0.0;
    for(int ii=0; ii<nGP; ii++)
      vals2project[0] += outval[ii];

    vals2project[0] /= nGP;

    return;
}


void Elem_Solid_2D_QUAD4::projectToNodes(bool extrapolateFlag, int vartype, int varindex, int index)
{
   double outval[50];

   switch(vartype)
   {
       case 1:  // plot total strain
       case 2:  // plot elastic strain
       case 3:  // plot plastic strain

                projectStrain(extrapolateFlag, vartype, varindex, index, outval);

              break;

       case 4:  // plot stress

                projectStress(extrapolateFlag, vartype, varindex, index, outval);

              break;

       case 5:  // plot element internal variables

                projectInternalVariable(extrapolateFlag, vartype, varindex, index, outval);

              break;

       default:

              cout  << "           Invalid Variable Type to project " << endl;
              break;
    }


    assert(vals2project.size() == npElem);
    stressrecovery_extrapolate_Quadrilateral(npElem, outval, &vals2project[0]);

  return;
}


void Elem_Solid_2D_QUAD4::projectStress(bool extrapolateFlag, int vartype, int varindex, int index, double* outval)
{
    int   err,  isw,  count,  count1, ll, ii, jj, gp;

    double  detF, fact, dvol, dvol0;
    double  Jac, bb1, bb2, bb3, cc1, cc2, cc3;
    double  param[2];

    VectorXd  N(npElem), dN_dx(npElem), dN_dy(npElem);
    double xNode[npElem], yNode[npElem], xx, yy;
    for(ii=0;ii<npElem;ii++)
    {
      xNode[ii] = GeomData->NodePosOrigLocal[nodeInLocalElem[ii]][0];
      yNode[ii] = GeomData->NodePosOrigLocal[nodeInLocalElem[ii]][1];
    }

    vector<double>  gausspoints1, gausspoints2, gaussweights;
    nGP = 4;
    getGaussPointsQuad(nGP, gausspoints1, gausspoints2, gaussweights);

    int    sss   = GeomData->getModelBehaviour();
    bool   axsy  = (sss == 3);
    bool  finite = MatlData->isFiniteStrain();

    MatrixXd  F(3,3);
    F.setZero();
    VectorXd  stre(9);
    stre.setZero();

    for(gp=0; gp<nGP; gp++)
    {
        param[0] = gausspoints1[gp];
        param[1] = gausspoints2[gp];

        GeomData->computeBasisFunctions2D(CONFIG_ORIGINAL, ELEM_SHAPE_QUAD, param, nodeNums, &N(0), &dN_dx(0), &dN_dy(0), Jac);

        computeDefGrad2DCur(dN_dx, dN_dy, F);

        detF = F(0,0)*F(1,1) - F(0,1)*F(1,0);

        // ADJUST F33 fOR 2D PROBLEMS BASED ON THE ASSUMPTIONS OF PLANE STRESS/PLANE STRAIN/AXISYMMETRIC
        adjust_deformationgradient_2D(sss, finite, F);

        MatlData->computeMechanicalStress(sss, F, F, pres, stre, ivar, gp, myTime.dt);

        if(varindex < 5)
           outval[gp] = stre[varindex];
        else if(varindex == 6)
           outval[gp] = sqrt((pow(stre[0]-stre[1],2.0) + pow(stre[1]-stre[2], 2.0) + pow(stre[2]-stre[0], 2.0) + 6* stre[3]*stre[3])/2);
        else if(varindex == 7)
           outval[gp] = (stre[0]+stre[1]+stre[2])/3.0;
    } //gp

    return;
}



void Elem_Solid_2D_QUAD4::projectStrain(bool extrapolateFlag, int vartype, int varindex, int index, double* outval)
{
  return;
}



void Elem_Solid_2D_QUAD4::projectInternalVariable(bool extrapolateFlag, int vartype, int varindex, int index, double* outval)
{
  return;
}




void  Elem_Solid_2D_QUAD4::computeEnergy(int index1, int index2, VectorXd& energy)
{
    // compute total energy for structural dynamic problems
    ///////////////////////////////////////////////////////////

    return;
}





